#!/usr/bin/env python

__author__ = 'flier'

import roslib
roslib.load_manifest('leap')

import rospy
import leap_interface
from leap.msg import leap
from leap.msg import leapros

# Obviously, this method publishes the data defined in leapros.msg to /leapmotion/data
def sender():
    li = leap_interface.Runner()
    li.setDaemon(True)
    li.start()
    # pub     = rospy.Publisher('leapmotion/raw',leap)
    pub_ros   = rospy.Publisher('leapmotion/data',leapros)
    rospy.init_node('leap_pub')
    available = True;
    while not rospy.is_shutdown():
        if li.has_new_information():
	    available = True;
	    nfingers	      = li.get_nfingers()
            hand_direction_   = li.get_hand_direction()
            hand_normal_      = li.get_hand_normal()
            hand_palm_pos_    = li.get_hand_palmpos()
            hand_pitch_       = li.get_hand_pitch()
            hand_roll_        = li.get_hand_roll()
            hand_yaw_         = li.get_hand_yaw()
            msg = leapros()
	    msg.nfingers = nfingers
            msg.direction.x = hand_direction_[0]
            msg.direction.y = hand_direction_[1]
            msg.direction.z = hand_direction_[2]
            msg.normal.x = hand_normal_[0]
            msg.normal.y = hand_normal_[1]
            msg.normal.z = hand_normal_[2]
            msg.palmpos.x = hand_palm_pos_[0]
            msg.palmpos.y = hand_palm_pos_[1]
            msg.palmpos.z = hand_palm_pos_[2]
            msg.ypr.x = hand_yaw_
            msg.ypr.y = hand_pitch_
            msg.ypr.z = hand_roll_
            pub_ros.publish(msg)
	else:
	    if available: 
		#print "no info available"
		available = False;
		msg = leapros()
		msg.nfingers = 0
		msg.direction.x = 0
		msg.direction.y = 0
		msg.direction.z = 0
		msg.normal.x = 0
		msg.normal.y = 0
		msg.normal.z = 0
		msg.palmpos.x = 0
		msg.palmpos.y = 0
		msg.palmpos.z = 0
		msg.ypr.x = 0
		msg.ypr.y = 0
		msg.ypr.z = 0
		pub_ros.publish(msg)

        # Save some CPU time, circa 100Hz publishing.
        rospy.sleep(0.1)


if __name__ == '__main__':
    try:
        sender()
    except rospy.ROSInterruptException:
        pass
